﻿using System;
using AForge.Robotics.Lego;

namespace NxtSofor
{
    class LegoDriver : ILegoDriver
    {
        private NXTBrick nxtBrick = new NXTBrick();
        private string COMPort;

        private int lastLeftMotorPower = 0;
        private int lastRightMotorPower = 0;

        public LegoDriver(string COMPort)
        {
            this.COMPort = COMPort;
        }

        public bool Connect()
        {
            if (!nxtBrick.Connect(COMPort))
            {
                return false;
            }

            nxtBrick.PlayTone(300, 300);
            return true;
        }

        public void Disconnect()
        {
            nxtBrick.PlayTone(150, 300);
            nxtBrick.Disconnect();
        }

        public void SetMotors(
            string leftMotorPort, 
            string rightMotorPort, 
            int leftMotorP, 
            int rightMotorP,
            int motorTurnRatio,
            int motorTachoLimit)
        {
            int maxMotorPower = 100;
            int leftMotorPower = 0;
            int rightMotorPower = 0;

            if (leftMotorP != 0)
            {
                if (leftMotorP > maxMotorPower)
                {
                    leftMotorPower = maxMotorPower;
                }
                else
                {
                    leftMotorPower = leftMotorP;
                }
            }

            if (rightMotorP != 0)
            {
                if (rightMotorP > maxMotorPower)
                {
                    rightMotorPower = maxMotorPower;
                }
                else
                {
                    rightMotorPower = rightMotorP;
                }
            }

            if ((leftMotorPower == lastLeftMotorPower) &&
                (rightMotorPower == lastRightMotorPower))
            {
                return;
            }

            if ((leftMotorPower == 0) && (rightMotorPower == 0))
            {
                NXTBrick.MotorState motorsState = new NXTBrick.MotorState();

                motorsState.Mode = NXTBrick.MotorMode.None;
                motorsState.RunState = NXTBrick.MotorRunState.Idle;

                nxtBrick.SetMotorState(NXTBrick.Motor.All, motorsState);
            }
            else
            {
                NXTBrick.MotorState leftMotorState = new NXTBrick.MotorState();
                NXTBrick.MotorState rightMotorState = new NXTBrick.MotorState();

                leftMotorState.Mode = NXTBrick.MotorMode.On;
                leftMotorState.RunState = NXTBrick.MotorRunState.Running;
                leftMotorState.Regulation = NXTBrick.MotorRegulationMode.Sync;
                leftMotorState.TachoLimit = motorTachoLimit; //0
                leftMotorState.Power = leftMotorPower;
                leftMotorState.TurnRatio = motorTurnRatio; //100

                rightMotorState.Mode = NXTBrick.MotorMode.On;
                rightMotorState.RunState = NXTBrick.MotorRunState.Running;
                rightMotorState.Regulation = NXTBrick.MotorRegulationMode.Sync;
                rightMotorState.TachoLimit = motorTachoLimit;
                rightMotorState.Power = rightMotorPower;
                rightMotorState.TurnRatio = motorTurnRatio;


                if (leftMotorPort == "Port A" && rightMotorPort == "Port B")
                {
                    nxtBrick.SetMotorState(NXTBrick.Motor.A, leftMotorState);
                    nxtBrick.SetMotorState(NXTBrick.Motor.B, rightMotorState);
                }
                else if (leftMotorPort == "Port A" & rightMotorPort == "Port C")
                {
                    nxtBrick.SetMotorState(NXTBrick.Motor.A, leftMotorState);
                    nxtBrick.SetMotorState(NXTBrick.Motor.C, rightMotorState);
                }
                else if (leftMotorPort == "Port B" & rightMotorPort == "Port A")
                {
                    nxtBrick.SetMotorState(NXTBrick.Motor.B, leftMotorState);
                    nxtBrick.SetMotorState(NXTBrick.Motor.A, rightMotorState);
                }
                else if (leftMotorPort == "Port B" & rightMotorPort == "Port C")
                {
                    nxtBrick.SetMotorState(NXTBrick.Motor.B, leftMotorState);
                    nxtBrick.SetMotorState(NXTBrick.Motor.C, rightMotorState);
                }
                else if (leftMotorPort == "Port C" & rightMotorPort == "Port A")
                {
                    nxtBrick.SetMotorState(NXTBrick.Motor.C, leftMotorState);
                    nxtBrick.SetMotorState(NXTBrick.Motor.A, rightMotorState);
                }
                else if (leftMotorPort == "Port C" & rightMotorPort == "Port B")
                {
                    nxtBrick.SetMotorState(NXTBrick.Motor.C, leftMotorState);
                    nxtBrick.SetMotorState(NXTBrick.Motor.B, rightMotorState);
                }
            }

            lastLeftMotorPower = leftMotorPower;
            lastRightMotorPower = rightMotorPower;
        }

        public bool GetInformation(out string deviceName, out byte[] btAddress, out int flashMemory, out int batteryLevel)
        {
            int btSignalStrength;
            if (nxtBrick.GetDeviceInformation(out deviceName, out btAddress, out btSignalStrength, out flashMemory) && nxtBrick.GetBatteryPower(out batteryLevel))
            {
                return true;
            }
            else
            {
                batteryLevel = -1;
                return false;
            }
        }

        public bool SetDeviceName(string deviceName)
        {
            if (nxtBrick.SetBrickName(deviceName))
            {
                return true;
            }
            else 
            {
                return false;
            }
        
        }


    }
}
